#!/usr/bin/env python
# -*- coding: utf-8 -*-

import rospy
import actionlib
from ros1_protobuf_msg_bridge.action import ActDemoAction
from ros1_protobuf_msg_bridge.action import ActDemoGoal,ActDemoFeedback,ActDemoResult

class ActDemoActionServer(object):
    def __init__(self):
        self._as = actionlib.SimpleActionServer(
            't_act1',
            ActDemoAction,
            execute_cb=self.execute_cb,
            auto_start=False)
        self._as.start()

    def execute_cb(self, goal):
        print(type(goal))
        print("Got goal: " + goal.pb.key + " " + str(goal.pb.val))
  
        #   发送状态数据
        feedback = ActDemoFeedback()
        feedback.pb.key = "Processing"
        feedback.pb.val = goal.pb.val
        feedback.pb.val3 = 0.0
        feedback.pb.val4 = 0.0

        for i in range(10):
            rospy.sleep(1.0)
            feedback.pb.val3 += 1.0
            feedback.pb.val4 += 2.0
            feedback.pb.val = i
            self._as.publish_feedback(feedback)
            print("feedback: " + str(feedback.pb.val))

        #  发送结果数据
        result = ActDemoResult()
        result.pb.key = goal.pb.key
        result.pb.val = goal.pb.val
        result.pb.val3 = feedback.pb.val3
        result.pb.val4 = feedback.pb.val4

        self._as.set_succeeded(result)
        print("Result: " + result.pb.key + " " + str(result.pb.val))

if __name__ == '__main__':
    rospy.init_node('py_act_server')
    server = ActDemoActionServer()
    rospy.spin()
